#include "bewegen.h"

#include <stdio.h>


/*
-------------------------------------------------------
Funktion "void bewegen.." führt die Bewegung des Roboters,
die der Werkzeuge sowie die der Bleche aus
-------------------------------------------------------
xe: x-Koordinate der Endstellung des Roboterarms
ye: y-Koordinate der Endstellung des Roboterarms
g: Greifervariable	*0: kein Werkzeug im Greifer
					*1: offene Hand
					*2: geschlossene Hand
					*3: Schweißgerät
xse: x-Koordinate der anzufahrenden Schienenposition
t_max: Zeitdauer der Roboterbewegung in [ms]
f1,f2: Fließbandvariablen	*0: Blech im Greifer
							*1: Blech auf Eingangsfließband
							*2: Blech auf Ausgangsfließband
					
*/

void bewegen(HWND hwnd, HDC hdc, RECT rect, double xe, double ye, int g, double xse, int t_max, int f1, int f2)
{
	Liste *runner, *altrunner;
	//double fixme[2], altfixme[2], statfixme[2], verfixme[2];
	double statfixme[2];
	int anz_loes;
	int t;
	int t_step = 40;			// dt in ms
	double xsa;				// x-Ausgangsposition des Roboters auf der Schiene
//	double sollpunkt[2];
	Punkt sollpunkt;
	double xa, ya;
	double winkel1[2], winkel2[2];
	//double verschiebung[2];
	Punkt verschiebung;

	xa = getElem(robo2_n, 9)->x;		//Festsetzen von Anfangspostion des Roboters
	ya = getElem(robo2_n, 9)->y;

	xsa = getElem(robo1_n, 4)->x;	//Festsetzen von Schienenposition des Roboters


/*Roboterarmtransfomation:	*/

	for (t=t_step; t<=t_max; t=t+t_step)
	{
		sollpunkt.x= xa + t*(xe-xa-(xse-xsa))/(t_max) - getElem(robo1, 4)->x-xsa;
		sollpunkt.y= ya + t*(ye-ya)/(t_max) - getElem(robo1 ,4)->y-300;
		statfixme[0] = sollpunkt.x;
		statfixme[1] = sollpunkt.y;
		anz_loes=SuchenWinkel(statfixme, l, l, winkel1, winkel2);
		
		verschiebung.x = (t * (xse-xsa) / t_max) + xsa;
		verschiebung.y = 300;
 	 
		runner = robo1;
		altrunner = robo1_n;
		while (runner && altrunner) {		//Transformation 1. Arm
			TrafoPoint(getPunkt(runner), getPunkt(altrunner), winkel1[0]+180, getElem(robo1, 4), &verschiebung);
			//TrafoPoint(robo1[i], robo1_n[i], winkel1[0]+180, robo1[4], verschiebung);
			runner = getNext(runner);
			altrunner = getNext(altrunner);
		}

		runner = robo2;
		altrunner = robo2_n;
		while (runner && altrunner) {
			TrafoPoint(getPunkt(runner), getPunkt(altrunner), winkel2[0]+180, getElem(robo2, 8), getElem(robo1_n, 5));
			//TrafoPoint(robo2[i], robo2_n[i],  winkel2[0]+180,  robo2[8],  robo1_n[5]);
			runner = getNext(runner);
			altrunner = getNext(altrunner);
		}
		
		
		switch(g)			//Werkzeugtransformation
		{
		
		case 1:
			h=1;
		runner = hand;
		altrunner = hand_n;
		while (runner && altrunner) {
			TrafoPoint(getPunkt(runner), getPunkt(altrunner), winkel2[0]+180, getElem(hand, 18), getElem(robo2_n, 9));
			//TrafoPoint(hand[i], hand_n[i], winkel2[0]+180, hand[18], robo2_n[9]);
			runner = getNext(runner);
			altrunner = getNext(altrunner);
		}
			break;
	
		case 2:
			h=0;
		runner = hand;
		altrunner = hand_n;
		while (runner && altrunner) {
			TrafoPoint(getPunkt(runner), getPunkt(altrunner), winkel2[0]+180, getElem(hand, 18), getElem(robo2_n, 9));
			//TrafoPoint(hand[i], hand_n[i], winkel2[0]+180, hand[18], robo2_n[9]);
			runner = getNext(runner);
			altrunner = getNext(altrunner);
		}
			break;

		case 3:
		runner = schweisser;
		altrunner = schweisser_n;
		while (runner && altrunner) {
			TrafoPoint(getPunkt(runner), getPunkt(altrunner), winkel2[0]+180, getElem(schweisser, 0), getElem(robo2_n, 9));
			//TrafoPoint(schweisser[i], schweisser_n[i], winkel2[0]+180, schweisser[0], robo2_n[9]);
			runner = getNext(runner);
			altrunner = getNext(altrunner);
		}
			break;

		} //switch(g)..

	
		if(f1==0) {			//1. Blech im Greifer
			runner = blech1_n;
			altrunner = blech1;
			while (runner && altrunner)
			{
				getPunkt(runner)->x = getPunkt(altrunner)->x + getElem(robo2_n, 9)->x;
				getPunkt(runner)->y = getPunkt(altrunner)->y + getElem(robo2_n, 9)->y;
				runner = getNext(runner);
				altrunner = getNext(altrunner);
			}
		}
		else if (f1==1)	{	//1. Blech auf Eingangsfließband	
			runner = blech1_n;
			while (runner)
			{
				getPunkt(runner)->x += t_step * 350 / t_max;
				runner = getNext(runner);
			}
		}
		else if (f1==2)	{	//1. Blech auf Ausgangsfließband
			runner = blech1_n;
			while (runner)
			{
				getPunkt(runner)->x += t_step * 450 / t_max;
				runner = getNext(runner);
			}
		}	
		if(f2==0) {			//2. Blech im Greifer
			runner = blech2_n;
			altrunner = blech2;
			while (runner && altrunner)
			{
				getPunkt(runner)->x = getPunkt(altrunner)->x + getElem(robo2_n, 9)->x;
				getPunkt(runner)->y = getPunkt(altrunner)->y + getElem(robo2_n, 9)->y;
				runner = getNext(runner);
				altrunner = getNext(altrunner);
			}
		}
		else if (f2==1)	{	//2. Blech auf Eingangsfließband
			runner = blech2_n;
			while (runner)
			{
				getPunkt(runner)->x += t_step * 350 / t_max;
				runner = getNext(runner);
			}
		}	
		else if (f2==2)	{	//2. Blech auf Ausgangsfließband
			runner = blech2_n;
			while (runner)
			{
				getPunkt(runner)->x += t_step * 450 / t_max;
				runner = getNext(runner);
			}
		}	

		InvalidateRect(hwnd, NULL, TRUE);
		hdc = BeginPaint (hwnd, &ps);
		 
		Zeichne_Statische_Elemente(hdc);
		Zeichne_Dynamische_Elemente(hdc);
		
		Sleep(t_step);


	} //for t..

} //void bewegen..
